<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "https://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en-US">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=11"/>
<meta name="generator" content="Doxygen 1.14.0"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>DM-CtrlH7-BF-DevProgram: C:/Users/ASUS/Desktop/dm-ctrlH7-balance-9025test/modules/imu/ins_task.c File Reference</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<script type="text/javascript" src="clipboard.js"></script>
<link href="navtree.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="navtreedata.js"></script>
<script type="text/javascript" src="navtree.js"></script>
<script type="text/javascript" src="cookie.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr id="projectrow">
  <td id="projectlogo"><img alt="Logo" src="50x5.png"/></td>
  <td id="projectalign">
   <div id="projectname">DM-CtrlH7-BF-DevProgram<span id="projectnumber">&#160;beta 0.1</span>
   </div>
   <div id="projectbrief">C.ONE Studio Damiao Development Board Framework</div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.14.0 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search/",'.html');
</script>
<script type="text/javascript">
$(function() { codefold.init(); });
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
$(function() {
  initMenu('',true,false,'search.php','Search',true);
  $(function() { init_search(); });
});
</script>
<div id="main-nav"></div>
</div><!-- top -->
<div id="side-nav" class="ui-resizable side-nav-resizable">
  <div id="nav-tree">
    <div id="nav-tree-contents">
      <div id="nav-sync" class="sync"></div>
    </div>
  </div>
  <div id="splitbar" style="-moz-user-select:none;" 
       class="ui-resizable-handle">
  </div>
</div>
<script type="text/javascript">
$(function(){initNavTree('ins__task_8c.html','',''); });
</script>
<div id="container">
<div id="doc-content">
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<div id="MSearchResults">
<div class="SRPage">
<div id="SRIndex">
<div id="SRResults"></div>
<div class="SRStatus" id="Loading">Loading...</div>
<div class="SRStatus" id="Searching">Searching...</div>
<div class="SRStatus" id="NoMatches">No Matches</div>
</div>
</div>
</div>
</div>

<div class="header">
  <div class="headertitle"><div class="title">ins_task.c File Reference</div></div>
</div><!--header-->
<div class="contents">
<div class="textblock"><code>#include &quot;<a class="el" href="ins__task_8h_source.html">ins_task.h</a>&quot;</code><br />
<code>#include &quot;<a class="el" href="controller_8h_source.html">controller.h</a>&quot;</code><br />
<code>#include &quot;<a class="el" href="_quaternion_e_k_f_8h_source.html">QuaternionEKF.h</a>&quot;</code><br />
<code>#include &quot;<a class="el" href="spi_8h_source.html">spi.h</a>&quot;</code><br />
<code>#include &quot;<a class="el" href="tim_8h_source.html">tim.h</a>&quot;</code><br />
<code>#include &quot;<a class="el" href="user__lib_8h_source.html">user_lib.h</a>&quot;</code><br />
<code>#include &quot;general_def.h&quot;</code><br />
</div><table class="memberdecls">
<tr class="heading"><td colspan="2"><h2 id="header-func-members" class="groupheader"><a id="func-members" name="func-members"></a>
Functions</h2></td></tr>
<tr class="memitem:a5474fb69eaecdc5d0837dbf9790eb72c" id="r_a5474fb69eaecdc5d0837dbf9790eb72c"><td class="memItemLeft" align="right" valign="top"><a id="a5474fb69eaecdc5d0837dbf9790eb72c" name="a5474fb69eaecdc5d0837dbf9790eb72c"></a>
<a class="el" href="structattitude__t.html">attitude_t</a> *&#160;</td><td class="memItemRight" valign="bottom"><b>INS_Init</b> (void)</td></tr>
<tr class="memdesc:a5474fb69eaecdc5d0837dbf9790eb72c"><td class="mdescLeft">&#160;</td><td class="mdescRight">初始化惯导解算系统 <br /></td></tr>
<tr class="memitem:a79b7240e8c7e0d5419282ca230c7a85e" id="r_a79b7240e8c7e0d5419282ca230c7a85e"><td class="memItemLeft" align="right" valign="top"><a id="a79b7240e8c7e0d5419282ca230c7a85e" name="a79b7240e8c7e0d5419282ca230c7a85e"></a>
void&#160;</td><td class="memItemRight" valign="bottom"><b>INS_Task</b> (void)</td></tr>
<tr class="memdesc:a79b7240e8c7e0d5419282ca230c7a85e"><td class="mdescLeft">&#160;</td><td class="mdescRight">此函数放入实时系统中,以1kHz频率运行 p.s. osDelay(1); <br /></td></tr>
<tr class="memitem:a151377c6cfa1152d7642d42cd7dd5d3d" id="r_a151377c6cfa1152d7642d42cd7dd5d3d"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="#a151377c6cfa1152d7642d42cd7dd5d3d">BodyFrameToEarthFrame</a> (const float *vecBF, float *vecEF, float *q)</td></tr>
<tr class="memdesc:a151377c6cfa1152d7642d42cd7dd5d3d"><td class="mdescLeft">&#160;</td><td class="mdescRight">Transform 3dvector from BodyFrame to EarthFrame.  <br /></td></tr>
<tr class="memitem:add6f841604e6b40025533d6e58d74ac8" id="r_add6f841604e6b40025533d6e58d74ac8"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="#add6f841604e6b40025533d6e58d74ac8">EarthFrameToBodyFrame</a> (const float *vecEF, float *vecBF, float *q)</td></tr>
<tr class="memdesc:add6f841604e6b40025533d6e58d74ac8"><td class="mdescLeft">&#160;</td><td class="mdescRight">Transform 3dvector from EarthFrame to BodyFrame.  <br /></td></tr>
<tr class="memitem:af69cef46084fc1f1f7f41a5e48f5a7e9" id="r_af69cef46084fc1f1f7f41a5e48f5a7e9"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="#af69cef46084fc1f1f7f41a5e48f5a7e9">QuaternionUpdate</a> (float *q, float gx, float gy, float gz, float dt)</td></tr>
<tr class="memdesc:af69cef46084fc1f1f7f41a5e48f5a7e9"><td class="mdescLeft">&#160;</td><td class="mdescRight">Update quaternion.  <br /></td></tr>
<tr class="memitem:a12a2c8b057bc73a18dc7765de080bd27" id="r_a12a2c8b057bc73a18dc7765de080bd27"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="#a12a2c8b057bc73a18dc7765de080bd27">QuaternionToEularAngle</a> (float *q, float *Yaw, float *Pitch, float *Roll)</td></tr>
<tr class="memdesc:a12a2c8b057bc73a18dc7765de080bd27"><td class="mdescLeft">&#160;</td><td class="mdescRight">Convert quaternion to eular angle.  <br /></td></tr>
<tr class="memitem:ab9879d429b498cb40bfb7027a771d12b" id="r_ab9879d429b498cb40bfb7027a771d12b"><td class="memItemLeft" align="right" valign="top">void&#160;</td><td class="memItemRight" valign="bottom"><a class="el" href="#ab9879d429b498cb40bfb7027a771d12b">EularAngleToQuaternion</a> (float Yaw, float Pitch, float Roll, float *q)</td></tr>
<tr class="memdesc:ab9879d429b498cb40bfb7027a771d12b"><td class="mdescLeft">&#160;</td><td class="mdescRight">Convert eular angle to quaternion.  <br /></td></tr>
</table><table class="memberdecls">
<tr class="heading"><td colspan="2"><h2 id="header-var-members" class="groupheader"><a id="var-members" name="var-members"></a>
Variables</h2></td></tr>
<tr class="memitem:ae8190ca21220538746a422536e9161af" id="r_ae8190ca21220538746a422536e9161af"><td class="memItemLeft" align="right" valign="top"><a id="ae8190ca21220538746a422536e9161af" name="ae8190ca21220538746a422536e9161af"></a>
const float&#160;</td><td class="memItemRight" valign="bottom"><b>xb</b> [3] = {1, 0, 0}</td></tr>
<tr class="memitem:a6cd4e6b14e528c9821cd1d45110d2cd9" id="r_a6cd4e6b14e528c9821cd1d45110d2cd9"><td class="memItemLeft" align="right" valign="top"><a id="a6cd4e6b14e528c9821cd1d45110d2cd9" name="a6cd4e6b14e528c9821cd1d45110d2cd9"></a>
const float&#160;</td><td class="memItemRight" valign="bottom"><b>yb</b> [3] = {0, 1, 0}</td></tr>
<tr class="memitem:a8766adb87d571c09bcfea9518b747cc9" id="r_a8766adb87d571c09bcfea9518b747cc9"><td class="memItemLeft" align="right" valign="top"><a id="a8766adb87d571c09bcfea9518b747cc9" name="a8766adb87d571c09bcfea9518b747cc9"></a>
const float&#160;</td><td class="memItemRight" valign="bottom"><b>zb</b> [3] = {0, 0, 1}</td></tr>
</table>
<a name="details" id="details"></a><h2 id="header-details" class="groupheader">Detailed Description</h2>
<div class="textblock"><dl class="section author"><dt>Author</dt><dd>Wang Hongxi </dd>
<dd>
annotation and modificaiton by neozng </dd></dl>
<dl class="section version"><dt>Version</dt><dd>V2.0.0 </dd></dl>
<dl class="section date"><dt>Date</dt><dd>2022/2/23 </dd></dl>
<dl class="section attention"><dt>Attention</dt><dd></dd></dl>
</div><a name="doc-func-members" id="doc-func-members"></a><h2 id="header-doc-func-members" class="groupheader">Function Documentation</h2>
<a id="a151377c6cfa1152d7642d42cd7dd5d3d" name="a151377c6cfa1152d7642d42cd7dd5d3d"></a>
<h2 class="memtitle"><span class="permalink"><a href="#a151377c6cfa1152d7642d42cd7dd5d3d">&#9670;&#160;</a></span>BodyFrameToEarthFrame()</h2>

<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void BodyFrameToEarthFrame </td>
          <td>(</td>
          <td class="paramtype">const float *</td>          <td class="paramname"><span class="paramname"><em>vecBF</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float *</td>          <td class="paramname"><span class="paramname"><em>vecEF</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float *</td>          <td class="paramname"><span class="paramname"><em>q</em></span>&#160;)</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>Transform 3dvector from BodyFrame to EarthFrame. </p>
<p>机体系到惯性系的变换函数</p>
<dl class="params"><dt>Parameters</dt><dd>
  <table class="params">
    <tr><td class="paramname">[1]</td><td>vector in BodyFrame </td></tr>
    <tr><td class="paramname">[2]</td><td>vector in EarthFrame </td></tr>
    <tr><td class="paramname">[3]</td><td>quaternion </td></tr>
  </table>
  </dd>
</dl>

</div>
</div>
<a id="add6f841604e6b40025533d6e58d74ac8" name="add6f841604e6b40025533d6e58d74ac8"></a>
<h2 class="memtitle"><span class="permalink"><a href="#add6f841604e6b40025533d6e58d74ac8">&#9670;&#160;</a></span>EarthFrameToBodyFrame()</h2>

<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void EarthFrameToBodyFrame </td>
          <td>(</td>
          <td class="paramtype">const float *</td>          <td class="paramname"><span class="paramname"><em>vecEF</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float *</td>          <td class="paramname"><span class="paramname"><em>vecBF</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float *</td>          <td class="paramname"><span class="paramname"><em>q</em></span>&#160;)</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>Transform 3dvector from EarthFrame to BodyFrame. </p>
<p>惯性系转换到机体系</p>
<dl class="params"><dt>Parameters</dt><dd>
  <table class="params">
    <tr><td class="paramname">[1]</td><td>vector in EarthFrame </td></tr>
    <tr><td class="paramname">[2]</td><td>vector in BodyFrame </td></tr>
    <tr><td class="paramname">[3]</td><td>quaternion </td></tr>
  </table>
  </dd>
</dl>

</div>
</div>
<a id="ab9879d429b498cb40bfb7027a771d12b" name="ab9879d429b498cb40bfb7027a771d12b"></a>
<h2 class="memtitle"><span class="permalink"><a href="#ab9879d429b498cb40bfb7027a771d12b">&#9670;&#160;</a></span>EularAngleToQuaternion()</h2>

<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void EularAngleToQuaternion </td>
          <td>(</td>
          <td class="paramtype">float</td>          <td class="paramname"><span class="paramname"><em>Yaw</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float</td>          <td class="paramname"><span class="paramname"><em>Pitch</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float</td>          <td class="paramname"><span class="paramname"><em>Roll</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float *</td>          <td class="paramname"><span class="paramname"><em>q</em></span>&#160;)</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>Convert eular angle to quaternion. </p>
<p>ZYX欧拉角转换为四元数 </p>

</div>
</div>
<a id="a12a2c8b057bc73a18dc7765de080bd27" name="a12a2c8b057bc73a18dc7765de080bd27"></a>
<h2 class="memtitle"><span class="permalink"><a href="#a12a2c8b057bc73a18dc7765de080bd27">&#9670;&#160;</a></span>QuaternionToEularAngle()</h2>

<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void QuaternionToEularAngle </td>
          <td>(</td>
          <td class="paramtype">float *</td>          <td class="paramname"><span class="paramname"><em>q</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float *</td>          <td class="paramname"><span class="paramname"><em>Yaw</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float *</td>          <td class="paramname"><span class="paramname"><em>Pitch</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float *</td>          <td class="paramname"><span class="paramname"><em>Roll</em></span>&#160;)</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>Convert quaternion to eular angle. </p>
<p>四元数转换成欧拉角 ZYX </p>

</div>
</div>
<a id="af69cef46084fc1f1f7f41a5e48f5a7e9" name="af69cef46084fc1f1f7f41a5e48f5a7e9"></a>
<h2 class="memtitle"><span class="permalink"><a href="#af69cef46084fc1f1f7f41a5e48f5a7e9">&#9670;&#160;</a></span>QuaternionUpdate()</h2>

<div class="memitem">
<div class="memproto">
      <table class="memname">
        <tr>
          <td class="memname">void QuaternionUpdate </td>
          <td>(</td>
          <td class="paramtype">float *</td>          <td class="paramname"><span class="paramname"><em>q</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float</td>          <td class="paramname"><span class="paramname"><em>gx</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float</td>          <td class="paramname"><span class="paramname"><em>gy</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float</td>          <td class="paramname"><span class="paramname"><em>gz</em></span>, </td>
        </tr>
        <tr>
          <td class="paramkey"></td>
          <td></td>
          <td class="paramtype">float</td>          <td class="paramname"><span class="paramname"><em>dt</em></span>&#160;)</td>
        </tr>
      </table>
</div><div class="memdoc">

<p>Update quaternion. </p>
<p>四元数更新函数,即实现dq/dt=0.5Ωq </p>

</div>
</div>
</div><!-- contents -->
</div><!-- doc-content -->
<div id="page-nav" class="page-nav-panel">
<div id="page-nav-resize-handle"></div>
<div id="page-nav-tree">
<div id="page-nav-contents">
</div><!-- page-nav-contents -->
</div><!-- page-nav-tree -->
</div><!-- page-nav -->
</div><!-- container -->
<!-- start footer part -->
<div id="nav-path" class="navpath"><!-- id is needed for treeview function! -->
  <ul>
    <li class="navelem"><a href="dir_e05d7e2b1ecd646af5bb94391405f3b5.html">modules</a></li><li class="navelem"><a href="dir_2e31b7153fd862194fb2f72cd70431e1.html">imu</a></li><li class="navelem"><a href="ins__task_8c.html">ins_task.c</a></li>
    <li class="footer">Generated by <a href="https://www.doxygen.org/index.html"><img class="footer" src="doxygen.svg" width="104" height="31" alt="doxygen"/></a> 1.14.0 </li>
  </ul>
</div>
</body>
</html>
